﻿using System;
using System.Collections.Generic;
using System.Text;
using PotentialFieldDriver.robotrace.environment.implementation;
using Geometry;
using PotentialFieldDriver.robotrace.utils.pitch;
using PotentialFieldDriver.robotrace.environment.interfaces;
using PotentialFieldDriver.robotrace.utils.fields;

namespace PotentialFieldDriver.robotrace.drivers.implementation
{
    class PotentialFieldBasedDriver : RobotDriverInterface
    {
        private const int DEFAILT_NUMBER_OF_ANGLES = 8;
        private const int DEFAILT_NUMBER_OF_VELOCITY_LEVELS = 20;
        private const double DEFAILT_MAX_ROBOT_SPEED = 1.0;
        private PitchInterface pitch;
        private volatile List<Point2D> optimalPath;
        private List<PotentialField> potentialFields;
        private double maxRobotSpeed = DEFAILT_MAX_ROBOT_SPEED;
        private double[] angles;
        private double[] velocities;
        private int wallRadix;
        private int currentCheckpoint = 0;
        private double maxVelocity = 1;
        // parameter stating how much speed can change in one step
        // given in fraction of the max speed
        private double velocityChangeInertiaParameter = 0.25;
        private int pathLength = 0;

        public PotentialFieldBasedDriver(PitchInterface pitch, List<Point2D> optimalPath, int wallRadix)
        {
            this.pitch = pitch;
            this.potentialFields = new List<PotentialField>();
            this.wallRadix = wallRadix;
            preparePotentialFields(optimalPath);
            this.optimalPath = optimalPath;
            this.pathLength = optimalPath.Count;
        }

        public void setWallRadix(int wallRadix)
        {
            this.wallRadix = wallRadix;
            preparePotentialFields(optimalPath);
        }

        private void preparePotentialFields(List<Point2D> path)
        {
            Console.WriteLine("Preparing potential fields");
            potentialFields.Clear();
            lock (path)
            {
                foreach (Point2D point in path)
                {
                    potentialFields.Add(new PotentialField(pitch, point, wallRadix));
                }
            }
            pathLength = potentialFields.Count;
            Console.WriteLine("Done preparing potential fields (" + potentialFields.Count + " fields created)");

            /*angles = new double[noOfAngles];
            for (int i = 0; i<noOfAngles; i++){
                angles[i] = generatAngle(i, noOfAngles);
            }
            velocities = new double[noOfVelocityLevels];
            for (int i = 0; i < noOfVelocityLevels; i++)
            {
                velocities[i] = generateVelocity(i, noOfVelocityLevels);
            }
            Cell[][] obstaclesBitmap = pitch.getObstaclesBitmap();
            potentialField = new PotentialFieldCell[obstaclesBitmap.Length][][][];
            for (int i = 0; i < obstaclesBitmap.Length; i++)
            {
                potentialField[i] = new PotentialFieldCell[obstaclesBitmap[i].Length][][];
                for (int j = 0; j < obstaclesBitmap[i].Length; j++)
                {
                    potentialField[i][j] = new PotentialFieldCell[noOfAngles][];
                    for (int k = 0; k < noOfAngles; k++)
                    {
                        potentialField[i][j][k] = new PotentialFieldCell[noOfVelocityLevels];
                        for (int m = 0; m < noOfVelocityLevels; m++)
                        {
                            PotentialFieldCell newCell = new PotentialFieldCell();
                            newCell.PitchCell = obstaclesBitmap[i][j];
                            newCell.Angle = generatAngle(k, noOfAngles);
                            newCell.Velocity = generateVelocity(m, noOfVelocityLevels);
                            potentialField[i][j][k][m] = newCell;
                        }
                    }
                }
            }
            fillPotentialFieldValues(potentialField);*/
        }

        private double generateVelocity(int m, int noOfVelocityLevels)
        {
            return m * 2 * maxRobotSpeed / (noOfVelocityLevels - 1) - maxRobotSpeed;
        }

        private double generatAngle(int k, int noOfAngles)
        {
            return k * 2 * Math.PI / noOfAngles;
        }

        private void fillPotentialFieldValues(PotentialFieldCell[][][][] potentialField)
        {
            Random rand = new Random();
            for (int column = 0; column < potentialField.Length; column++)
            {
                for (int row = 0; row < potentialField[column].Length; row++)
                {
                    for (int angleIdx = 0; angleIdx < potentialField[column][row].Length; angleIdx++)
                    {
                        for (int velocityIdx = 0; velocityIdx < potentialField[column][row][angleIdx].Length; velocityIdx++)
                        {
                            //TODO: dodać jakieś sensowne pole
                            potentialField[column][row][angleIdx][velocityIdx].LeftSpeed = rand.NextDouble();
                            potentialField[column][row][angleIdx][velocityIdx].RightSpeed = rand.NextDouble();
                        }
                    }
                }
            }
        }

        public void step(FIRADriverLibrary.FIRARobotState robot)
        {
            if (pathLength != optimalPath.Count)
            {
                preparePotentialFields(optimalPath);
            }

            /*int angleIdx = findNearestAngle(robot.angle);
            int velocityIdx = findNearestVelocity((robot.leftSpeed + robot.rightSpeed) / 2);
            PotentialFieldCell currentCell = potentialField[cellColumn][cellRow][angleIdx][velocityIdx];
            robot.leftSpeed = currentCell.LeftSpeed;
            robot.rightSpeed = currentCell.RightSpeed;*/
            if (potentialFields.Count <= 0)
            {
                return;
            }
            if (currentCheckpoint > optimalPath.Count)
            {
                currentCheckpoint = 0;
            }
            double currentRobotXPosition = robot.x;
            double currentRobotYPosition = robot.y;
            double distanceFromNextCheckpoint = optimalPath[currentCheckpoint].GetDistance(currentRobotXPosition, currentRobotYPosition);

            while (robotReachedCheckpoint(distanceFromNextCheckpoint))
            {
                currentCheckpoint++;
                if (currentCheckpoint >= optimalPath.Count)
                {
                    currentCheckpoint = 0;
                }
                distanceFromNextCheckpoint = optimalPath[currentCheckpoint].GetDistance(currentRobotXPosition, currentRobotYPosition);
            }
            if (currentCheckpoint >= potentialFields.Count)
            {
                preparePotentialFields(optimalPath);
            }
            PotentialField field = potentialFields[currentCheckpoint];
            RobotCommand command = computeSpeeds(field, currentCheckpoint, robot);
            robot.leftSpeed = command.leftSpeed;
            robot.rightSpeed = command.rightSpeed;
        }

        private bool robotReachedCheckpoint(double distanceFromNextCheckpoint)
        {
            if (distanceFromNextCheckpoint <= 0.05)
            {
                return true;
            }
            return false;
        }

        private RobotCommand computeSpeeds(PotentialField field, int currentTarget, FIRADriverLibrary.FIRARobotState robot)
        {
            int cellColumn = pitch.mapRobotXPositionToCellColumnNo(robot.x);
            int cellRow = pitch.mapRobotYPositionToCellRowNo(robot.y);
            //double xGradient = field.getValue(cellColumn - 1, cellRow) - field.getValue(cellColumn + 1, cellRow);
            //double yGradient = field.getValue(cellColumn, cellRow - 1) - field.getValue(cellColumn, cellRow + 1);
            double currentRobotXPosition = robot.x;
            double currentRobotYPosition = robot.y;
            double currentRobotAngle = robot.angle;
            double currentRobotLeftSpeed = robot.leftSpeed;
            double currentRobotRightSpeed = robot.rightSpeed;

            Vector2D gradient = computeGradient(field, 1, cellColumn, cellRow);//new Vector2D(xGradient, yGradient);
            Vector2D robotMoveVector = new Vector2D(currentRobotAngle) * (1 + (currentRobotLeftSpeed + currentRobotRightSpeed) / 2);
            RobotCommand command = new RobotCommand();

            double differenceAngle = Vector2D.AngleBetween(robotMoveVector, gradient);
            Console.WriteLine(gradient + " ," + robotMoveVector + ", " + differenceAngle + ", left: " + currentRobotLeftSpeed + ", right: " + currentRobotRightSpeed);

            double maxGradient = Math.Max(Math.Abs(gradient.X), Math.Abs(gradient.Y));
            //command.leftSpeed = Math.Max(Math.Min((xGradient / 20.0) * maxVelocity, maxVelocity), -maxVelocity);
            //command.rightSpeed = Math.Max(Math.Min((yGradient / 20.0) * maxVelocity, maxVelocity), -maxVelocity);

            if (differenceAngle > -Math.PI && differenceAngle <= -Math.PI / 2)
            {
                command.rightSpeed = changeSpeed(currentRobotRightSpeed, -maxVelocity);
                command.leftSpeed = changeSpeed(currentRobotLeftSpeed, -maxVelocity * adjust(1 - (Math.PI + differenceAngle)));
            }
            else if (differenceAngle <= 0)
            {
                command.rightSpeed = changeSpeed(currentRobotRightSpeed, maxVelocity);
                command.leftSpeed = changeSpeed(currentRobotLeftSpeed, maxVelocity * adjust(Math.PI / 2 + differenceAngle));
            }
            else if (differenceAngle <= Math.PI / 2)
            {
                command.leftSpeed = changeSpeed(currentRobotLeftSpeed, maxVelocity);
                command.rightSpeed = changeSpeed(currentRobotRightSpeed, maxVelocity * adjust(Math.PI / 2 - differenceAngle));
            }
            else
            {
                command.leftSpeed = changeSpeed(currentRobotLeftSpeed, -maxVelocity);
                command.rightSpeed = changeSpeed(currentRobotRightSpeed, -maxVelocity * adjust(1 - (Math.PI - differenceAngle)));
            }
            return command;
        }

        private Vector2D computeGradient(PotentialField field, int radix, int column, int row)
        {
            double averageXLeft = 0;
            double averageXRight = 0;
            double averageYUp = 0;
            double averageYDown = 0;
            int count = 0;
            for (int i = 0; i < radix; i++)
            {
                for (int j = -radix; j < radix + 1; j++)
                {
                    count++;
                    averageXLeft += field.getValue(column - i-1, row + j);
                    averageXRight += field.getValue(column + i + 1, row + j);
                    averageYUp += field.getValue(column + j, row - i - 1);
                    averageYDown += field.getValue(column + j, row + i + 1);
                }
            }
            double averageX = (averageXLeft - averageXRight)/count;
            double averageY = (averageYUp - averageYDown)/count;
            return new Vector2D(averageX, averageY);
        }

        private Double adjust(Double d)
        {
            return Math.Abs(d / Math.PI * 2);
        }

        private double changeSpeed(double previousSpeed, double destinationSpeed)
        {
            double difference = Math.Abs(previousSpeed - destinationSpeed);
            double maxSpeedChange = maxVelocity * velocityChangeInertiaParameter;
            if (difference < maxSpeedChange || true)
            {
                return destinationSpeed;
            }
            double sign = previousSpeed > destinationSpeed ? -1.0 : 1.0;
            return previousSpeed + sign * maxSpeedChange;
        }

        private int findNearestVelocity(double p)
        {
            return findNearesValue(p, velocities);
        }

        private int findNearestAngle(double p)
        {
            return findNearesValue(p, angles);
        }

        private int findNearesValue(double val, double[] array)
        {
            double difference = double.MaxValue;
            int idx = 0;
            for (int i = 0; i < array.Length; i++)
            {
                double tmp = Math.Abs(array[i] - val);
                if (tmp < difference)
                {
                    difference = tmp;
                    idx = i;
                }
            }
            return idx;
        }


        public System.Drawing.Bitmap getBitmap()
        {
            if (currentCheckpoint >= potentialFields.Count)
            {
                return null;
            }
            return potentialFields[currentCheckpoint].PotentialBitmap;
        }
    }

    struct RobotCommand
    {
        public double leftSpeed;
        public double rightSpeed;
    }

    class PotentialFieldCell
    {
        private Cell pitchCell;
        private double angle;
        private double velocity;

        private double leftSpeed;
        private double rightSpeed;

        public double LeftSpeed
        {
            get;
            set;
        }

        public double RightSpeed
        {
            get;
            set;
        }

        public Cell PitchCell
        {
            get;
            set;
        }

        public double Angle
        {
            get;
            set;
        }

        public double Velocity
        {
            get;
            set;
        }
    }

}
